#include "pi.h"

// 计算PI控制器输出
void PI_Controller_Ud(FOC_parame_ *pi, float Target)
{
    static float PreError;
    static float deltaOutput;
    // 当前误差
    float Error = Target - pi->I_d;
    // 计算增量PI输出
    deltaOutput += pi->Kp * (Error - PreError) + pi->Ki * Error;
    PreError = Error;
    // 更新PI输出
    pi->Ud = deltaOutput;

    // 输出限幅并进行抗饱和控制
    if (pi->Ud > Ud_limitUp)
    {
        pi->Ud = Ud_limitUp;
    }
    else if (pi->Ud < Ud_limitDown)
    {
        pi->Ud = Ud_limitDown;
    }
}
void PI_Controller_Uq(FOC_parame_ *pi, float Target)
{
    static float PreError;
    static float deltaOutput;
    // 当前误差
    float Error = Target - pi->I_q;
    // 计算增量PI输出
    deltaOutput += pi->Kp * (Error - PreError) + pi->Ki * Error;
    PreError = Error;
    // 更新PI输出
    pi->Uq = deltaOutput;

    // 输出限幅并进行抗饱和控制
    if (pi->Uq > Uq_limitUp)
    {
        pi->Uq = Uq_limitUp;
    }
    else if (pi->Uq < Uq_limitDown)
    {
        pi->Uq = Uq_limitDown;
    }
}
